/* Include files */
#include "Aria.h"
#include <stdio.h>
#include <math.h>
#include <iostream>
using namespace std;
void wander(double speed, ArRobot *thisRobot);

/* Main method */

int main2(int argc, char **argv) {
	/* The robot and its devices */
	Aria::init(); //Initialise ARIA library
	ArRobot robot; //Instantiate robot

	ArSonarDevice sonar; //Instantiate its sonar
	robot.addRangeDevice(&sonar); //Add sonar to robot

	ArArgumentParser parser(&argc, argv); //Instantiate argument parser
	ArSimpleConnector connector(&parser); //Instantiate connector
	/* Connection to robot */
	parser.loadDefaultArguments(); //Load default values
	if (!connector.parseArgs()) //Parse connector arguments
	{
		cout << "Unknown settings\n"; //Exit for errors
		Aria::exit(0);
		exit(1);
	}
	if (!connector.connectRobot(&robot)) //Connect to the robot
	{
		cout << "Unable to connect\n"; //Exit for errors
		Aria::exit(0);
		exit(1);
	}
	robot.runAsync(true); //Run in asynchronous mode
	robot.lock(); //Lock robot during set up
	robot.comInt(ArCommands::ENABLE, 1); //Turn on the motors
	robot.unlock(); //Unlock the robot


	double initX = 0;
	double initY = 0;
	double finX = 4000;
	double finY = 2000;

	double m = (finY - initY) / (finX - initY);
	double degree = atan(m) * 57.32;
	double dist = sqrt((finX - initX) * (finX - initX) + (finY - initY) * (finY
			- initY));

	//robot.setVel(50);
	robot.setRotVel(10);
	robot.setHeading(degree);
	robot.setVel(50);
	robot.move(dist);

	robot.waitForRunExit(0);
	Aria::exit(0); //Exit Aria
} //End main
